#include <avr/interrupt.h>

char servo = 0;

ISR(TIMER1_COMPA_vect)
{
//	DDRD |= 1<<7;
//	PORTD ^= 1<<7;
}
ISR(TIMER1_COMPB_vect)
{
//	DDRD |= 1<<7;
//	PORTD ^= 1<<7;
}
ISR(TIMER1_COMPC_vect)
{
//	DDRD |= 1<<7;
//	PORTD ^= 1<<7;
}





void Servo_Init()
{
	OCR1A  = (unsigned int)(1.5 * ( F_CPU / 256000));       
	OCR1B  = (unsigned int)(1.5 * ( F_CPU / 256000));       
	OCR1C  = (unsigned int)(1.5 * ( F_CPU / 256000)); 
    
	TCCR1A = (1<<COM1A1) | (1<<COM1B1) | (1<<COM1C1);
    TCCR1B = (1 << CS12);    // clk / 256                    

	//set wavemode, top 0x3ff
	TCCR1A |= (1<<WGM10) | (1<<WGM11);
	TCCR1B |= (1<<WGM12) | (0<<WGM13);

	//TIMSK |= (1<<OCIE1A)|(1<<OCIE1B);//|(TOIE1);
	//ETIMSK |= (1<<OCIE1C);
	TCNT1 = 0;

	DDRB |= (1<<PB5) | (1<<PB6) | (1<<PB7);
}


void PWM_Init()
{
	OCR3A  = 64;       
	OCR3B  = 128;       
	OCR3C  = 192; 
	 
	//Set OC0 on compare match when up-counting. Clear OC0 on compare match when downcounting.
	TCCR3A = (1<<COM3A1) | (1<<COM3A0) | (1<<COM3B1) | (1<<COM3B0) | (1<<COM3C1) | (1<<COM3C0);
	
	// set speed
	TCCR3B = (1 << CS30);  

	//PWM, Phase Correct, 8-bit
	TCCR3A |= (1<<WGM30);
	TCCR3B |= 0;

	//ETIMSK |= (1<<OCIE3A)|(1<<OCIE3B)|(1<<OCIE3C)|(1<<TOIE3);
	TCNT3 = 0;

	DDRE |= (1<<PE3) | (1<<PE4) | (1<<PE5);
}

void PWM_Set( unsigned char index, unsigned char pos )
{
	if (index == 0)
	{
		OCR3AH = 0;
		OCR3AL = pos;
	}
	else if (index == 1)
	{
		OCR3BH = 0;
		OCR3BL = pos;
	}
	else if (index == 2)
	{
		OCR3CH = 0;
		OCR3CL = pos;
	}
}


void Servo_Set1( unsigned char index, unsigned char pos )
{
	if (index == 0)
	{
		OCR1AH = 0;
		OCR1AL = pos;
	}
	else if (index == 1)
	{
		OCR1BH = 0;
		OCR1BL = pos;
	}
	else if (index == 2)
	{
		OCR1CH = 0;
		OCR1CL = pos;
	}
}



void Servo_Print()
{
	PrintHex16( OCR1A );
	Device_PrintCRLF();		

	PrintHex16( OCR1B );
	Device_PrintCRLF();		

	PrintHex16( OCR1C );
	Device_PrintCRLF();		
}

